import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64,Bool
from sensor_msgs.msg import LaserScan
from mower_msgs.msg import Supersonic


class Ultrasonic(Node):
    def __init__(self):
        super().__init__('ultrasonic_node')
        
        self.ranges = []

        # 订阅刀盘电机控制命令
        self.ultrasonic_scan_sub = self.create_subscription(
            LaserScan, 'ultrasonic_scan', self.ultrasonic_scan_callback, 10)
        
        # 发布超声波数据
        self.supersonic_publisher = self.create_publisher(Supersonic, 'Distance', 10)

        # 创建定时器，每0.05秒发布一次超声波数据
        self.supersonic_timer = self.create_timer(0.05, self.publish_supersonic)
        
    def ultrasonic_scan_callback(self, msg):
        self.ranges = msg.ranges
        for i in range(len(self.ranges)):
            if self.ranges[i] > 5:
                self.ranges[i] = 5
        
        # print(self.ranges)
    
    def publish_supersonic(self):
        
        supersonic_msg = Supersonic()
        supersonic_msg.header.stamp = self.get_clock().now().to_msg()
        supersonic_msg.header.frame_id = 'ultrasonic'
        if len(self.ranges) == 0:
            return
        supersonic_msg.distance_a = sum(self.ranges) / len(self.ranges)
        self.supersonic_publisher.publish(supersonic_msg)


def main(args=None):
    rclpy.init(args=args)
    node = Ultrasonic()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

